/**
 * Author: luoqi
 * Created Date: 2025-10-14 00:19:8
 * Last Modified: 2025-11-26 22:40:30
 * Modified By: luoqi at <**@****>
 * Copyright (c) 2025 <*****>
 * Description:
 */

#include "xbot.h"

XBot::XBot()
{
    if(canfd.open() < 0) {
        QLog::err("XBot::XBot: canfd open failed");
        return;
    }
    joints.port_set(
        [this](uint16_t id, void *data, uint8_t sz) -> int { return canfd.send(id, data, sz); },
        [this](uint16_t &id, void *buf, uint8_t &sz, int timeout_ms) -> int { return canfd.recv(id, buf, sz, timeout_ms); }
    );
}

XBot::~XBot()
{
    stop();
}

int XBot::start()
{
    running_ = true;
    thr_canfd_recv_ = std::thread(&XBot::task_canfd_recv_, this);
    thr_xbot_task_ = std::thread(&XBot::task_xbot_, this);
    return 0;
}

int XBot::stop()
{
    running_ = false;
    if(thr_canfd_recv_.joinable()) {
        thr_canfd_recv_.join();
    }
    canfd.close();
    if(thr_xbot_task_.joinable()) {
        thr_xbot_task_.join();
    }
    return 0;
}

int XBot::ena(EnaState state)
{
    if(state == ENABLE) {
        for(int i = 1; i <= joints.num(); i++) {
            joints.enable(i);
        }
    } else {
        for(int i = 1; i <= joints.num(); i++) {
            joints.disable(i);
            joints.mit_set(i, 0, 0, 0, 0, 0);
        }
    }
    return 0;
}

void XBot::task_canfd_recv_()
{
    int timeout = 0;
    while(running_) {
        uint16_t id;
        uint8_t sz;
        uint8_t data[64];
        int nbytes = canfd.recv(id, data, sz, 10);
        if(nbytes < 0) {
            QLog::err("XBot::task_canfd_recv: recv failed");
            break;
        } else if(nbytes == 0) {
            if(timeout++ > 10) {
                joints.poll();
                timeout = 0;
            }
            continue;
        }
        timeout = 0;
        if(dbg_canfd) {
            std::printf("id: 0x%04x, sz: %d\r\n", id, sz);
            for(int i = 0; i < sz; i++) {
                std::printf(" 0x%02x", data[i]);
            }
            std::printf("\r\n");
        }

        if(joints.decode(id, data, sz) < 0) {
            QLog::err("XBot::task_canfd_recv: decode failed, id: 0x{:04x}, sz: {}", id, sz);
            continue;
        }
    }
    QLog::info("XBot::task_canfd_recv: exit");
}

void XBot::task_xbot_()
{
    while(running_) {

        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
}
